/*
Veronda & Westmann proposed a function with exponential
nonlinearity, leaving the exact I3 dependency unspecified.

This function is based on the paper "An implicit FE method for 
elastic solids in contact by Gentaro Hirota et.al."

However, it is not consistent with the linear model.

W(I1,I2, I3) = 0.5 (mu/gamma (exp (gamma(I1 -3)) - 1) - mu/2 (I2 - 3) +
lambda (I3 - ln (I3)))


S =  [mu exp(gamma (I1-3)) I - mu/2 (I1 I - C) + lambda [I3 - 1] C^-1]

force = - F S X^-T

For the derivative we get

Dforce = - (DF S  + F DS) X^-T

where

DS = mu exp(gamma (I1-3)) gamma tr(DC) Id - mu/2 (tr(DC) Id. - DH)
+ lambda (I3 C^-1 : DC) C^-1 - lambda (I3-1) C^-1 DC C^-1

= (exp (..) gamma - 1/2) mu tr(DC) Id.+ mu/2 DH
+ lambda ( I3 C^-1 : DC - (I3-1) C^-1 DC) C^-1

*/

#include <cmath>

#include "TetrahedronState.h"
#include "Matrix.h"
#include "Mechanics.h"


Real VerondaElasticEnergy (Real I1, Real I2, Real I3)
{
	return 0.5*  (lame_mu/lame_gamma* (exp (lame_gamma*(I1 -3)) - 1) - lame_mu/2* (I2 - 3) +
		lame_lambda * (I3 - log (I3)));
}



/*
(+
Matrix3::multiply_mm 
Matrix3::add_diag 
Matrix3::square 
Matrix3::determinant 
1
Matrix3::trace 

Matrix3::invert_to_with_det 
250
5
Matrix3::diag
Matrix3::saxpy 
Matrix3::saxpy 
3
Matrix3::multiply_mm   Matrix3::multiply_mm
Matrix3::negate )

547
*/

int VerondaElasticForce (TetrahedronState const*me,
					   Matrix3 *force_mat,
					   Matrix3 const &disp_mat)
{
	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real I3 = Sqr(J);
	Real I1 = C.Trace ();

	Matrix3::InvertToWithDet (C_inv, C, I3);

	Matrix3 A;
	A.Diag (lame_mu*  (exp (lame_gamma*(I1 -3)) - 0.5*I1));
	Matrix3::Saxpy (A, lame_mu * 0.5, C, A);
	Matrix3::Saxpy (A,  lame_lambda * (I3 -1), C_inv, A);

	*force_mat = F * A * me->invloc_transpose_;
	Matrix3::Negate (*force_mat, *force_mat);

	return 547;
}


/*
(+
Matrix3::multiply_mm 
Matrix3::add_diag 

Matrix3::square 
Matrix3::determinant 
Matrix3::trace 

Matrix3::invert_to_with_det 


250
2
2

Matrix3::diag 
Matrix3::saxpy 
1
2
Matrix3::saxpy 
Matrix3::multiply_mm
Matrix3::multiply_mm 

Matrix3::multiply_mm 
Matrix3::multiply_mtm 

Matrix3::double_symmetrify 
4

Matrix3::diag
Matrix3::trace 
2
Matrix3::saxpy 

Matrix3::multiply_mtm
Matrix3::scale
2
Matrix3::add_diag 
Matrix3::multiply_mtm
Matrix3::scale 

Matrix3::add  
Matrix3::multiply_mtm   Matrix3::multiply_mtm   Matrix3::multiply_mtm
Matrix3::add
Matrix3::negate 

)

997
*/
int VerondaElasticForceDerivative (TetrahedronState const*me,
								  Matrix3 *dforce,
								  Matrix3 *force,
								  Matrix3 const &disp_mat,
								  Matrix3 const & dir_mat)
{
	Matrix3 C;
	Matrix3 C_inv;
	Matrix3 F;

	Matrix3::MultiplyMM (F, disp_mat, me->inverse_location_);
	F.AddDiag (1.0);

	Matrix3::Square (C, F);
	Real J = F.Determinant ();
	Real I3 = Sqr(J);
	Real I1 = C.Trace ();

	Matrix3::InvertToWithDet (C_inv, C, I3);

	Matrix3 A;
	Real exponential = exp (lame_gamma*(I1 -3));
	A.Diag (lame_mu* (exponential - I1/2));
	Matrix3::Saxpy (A, lame_mu / 2.0, C, A);
	Matrix3::Saxpy (A,  lame_lambda * (I3 -1), C_inv, A);

	*force = F * A * me->invloc_transpose_;

	Matrix3 DF;
	Matrix3::MultiplyMM (DF, dir_mat, me->inverse_location_);

	Matrix3 DC;
	Matrix3::MultiplyMTM (DC, F, DF);

	Matrix3::DoubleSymmetrify (DC, DC);	

	Matrix3 DA;

	DA.Diag( (exponential *  lame_gamma - 0.5) * lame_mu * DC.Trace ());

	Matrix3::Saxpy (DA, + lame_mu * 0.5 , DC, DA);

	Matrix3 D3 =  C_inv * DC ;
	Matrix3::Scale (D3, - (I3-1), D3);
	D3.AddDiag (I3 * Matrix3::InnerProduct (C_inv , DC));
	D3 = D3 * C_inv;
	Matrix3::Scale (D3, lame_lambda, D3);

	Matrix3::Add (DA, DA, D3); 

	*dforce = (DF * A + F * DA) * me->invloc_transpose_;
	Matrix3::Negate (*dforce, *dforce);


	int flops = 997;
	if (force)
	{
		*force = F * A * me->invloc_transpose_;
		Matrix3::Negate (*force, *force);

		flops  += 117;
	}

	return flops;  
}
